﻿// PointCloudTest.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

#include <iostream>
#include <boost/filesystem.hpp>

#include "../PiontCloudLib/GMM.h"
#include "../PiontCloudLib/KMeans.h"
#include "../PiontCloudLib/Spectral.h"
#include "../PiontCloudLib/Utility.h"

#include "../PiontCloudLib/KdTree.h"
#include "../PiontCloudLib/ModelFit.h"
#include <pcl/point_types.h>
#include <Open3D/Open3D.h>
#include <Eigen/Eigen>
#include "DBSCAN.h"

//#include <vtk-9.0/vtkac`>


std::vector<std::string> split(const std::string& str, const std::string& delims) {
    std::vector<std::string> output;
    auto first = std::cbegin(str);

    while (first != std::cend(str)) {
        const auto second = std::find_first_of(first, std::cend(str),
            std::cbegin(delims), std::cend(delims));

        if (first != second)
            output.emplace_back(first, second);

        if (second == std::cend(str))
            break;

        first = std::next(second);
    }

    return output;
}


void readKittiPclBinData(const std::string& in_file, std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d>>& points) {
	points.clear();
	std::fstream input(in_file.c_str(), std::ios::in | std::ios::binary);
	if (!input.good()) {
		std::cerr << "Could not read file: " << in_file << std::endl;
		exit(EXIT_FAILURE);
	}
	input.seekg(0, std::ios::beg);
	int i;
	for (i = 0; input.good() && !input.eof(); i++) {
		Eigen::Vector4f point;
		input.read((char*)&(point), 4 * sizeof(float));
		points.emplace_back(point.x(), point.y(), point.z());
	}
	input.close();
}



/**
 * 读取点云信息
 * @param file
 * @param points
 */
void ReadPointCloud(std::string file, std::vector<klh_utility::Point<float>>& points) {
    std::ifstream in(file.c_str());
    std::string temp_line;
    while (std::getline(in, temp_line)) {
        auto ss = split(temp_line, " ");
        points.emplace_back(std::atof(ss[0].c_str()), std::stof((ss[1].c_str())));
    }
}


//void LessonSecond()
//{
//	std::string file_path = "..//data//second//000000.bin";
//	auto point_cloud = std::make_shared<open3d::geometry::PointCloud>();
//	readKittiPclBinData(file_path, point_cloud);
//	open3d::visualization::DrawGeometries({point_cloud});
//	open3d::utility::Timer timer;
//	timer.Start();
//	KdTree tree(point_cloud->points_, 8);
//	timer.Stop();
//	std::cout << "build tree : " << timer.GetDuration() << std::endl;
//
//	KNNResult knn_result(8);
//	double alltime = 0;
//	for (const auto& p : point_cloud->points_) {
//		knn_result = KNNResult(8);
//		timer.Start();
//		tree.KNNSearch(p, knn_result);
//
//		timer.Stop();
//		//knn_result.print();
//		alltime += timer.GetDuration();
//	}
//	std::cout << "tree knn search : " << alltime / point_cloud->points_.size() << std::endl;
//
//
//}

void LessonThree()
{
	using directory = boost::filesystem::directory_iterator ;
	boost::filesystem::path path("..\\data\\third");
	directory entry(path);
	for(;entry != directory();++entry)
	{
        std::vector<klh_utility::Point<float>> points;
		
        ReadPointCloud(entry->path().string(), points);
        const int k = 3;

  //      klh_utility::StopWatch time;
  //      
		//KMeans kMeans(k, 300, 0.001);
  //      time.reset();
  //      kMeans.fit(points);
  //      std::cout << "time: " << time.getTime() << " ms" << std::endl;
  //      const auto indexs = kMeans.predict(points);

        GMM gmm(k,50);
        gmm.fit(points);
        auto indexs = gmm.predict(points);

        //Spectral spectral(k);
        //spectral.fit(points);
        //auto indexs = spectral.predict(points);
        klh_utility::ShowLaserData data(points);
        data.DrawClusterPoints(points, indexs);
        //showData(points, indexs, k);
	}
}


std::vector<std::vector<int>> 
	fourthBlock(
		const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& pc) 
{
	std::vector<std::vector<int>> res(4);
	Eigen::Vector3d mean(0,0,0);
	for (const auto& p : pc)
	{
		mean += p;
	}
	mean /= pc.size();

	for (int i =0 ;i < pc.size();i ++)
	{
		int site = 0;
		if (pc[i].x() < mean.x()) site = site | 1;
		if (pc[i].y() < mean.y()) site = site | 2;

		res[site].push_back(i);
	}
	return res;
}

std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>
getPoints(
	const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& pc, std::vector<int> is)
{
	std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> res;
	for (const auto& i : is)
		res.push_back(pc[i]);
	return res;
}



void LessonFourth() 
{
	using directory = boost::filesystem::directory_iterator;
	boost::filesystem::path path("..\\data\\fourth");
	directory entry(path);
	for (; entry != directory(); ++entry)
	{
		std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> points;
		readKittiPclBinData(entry->path().string(), points);


		auto fourth_ps = fourthBlock(points);

		//for (const auto& ps : fourth_ps)
		//{
		//	auto point_cloud_temp = std::make_shared<open3d::geometry::PointCloud>();
		//	for (const auto& i : ps)
		//	{
		//		point_cloud_temp->points_.push_back(points[i]);
		//	}
		//	open3d::visualization::DrawGeometries({ point_cloud_temp });
		//}
		auto point_cloud = std::make_shared<open3d::geometry::PointCloud>();

		std::vector<int> ground;
		for (const auto& ps : fourth_ps)
		{
			auto temp_p = getPoints(points, ps);
			auto temp_ground = calRansacNormal(temp_p, 0.2);
			for (const auto& t : temp_ground)
			{
				ground.push_back(ps[t]);
			}
			for (const auto& i : temp_ground)
			{
				point_cloud->points_.push_back(temp_p[i]);
				point_cloud->colors_.emplace_back(0, 0, 255);
			}
			open3d::visualization::DrawGeometries({ point_cloud });
		}

		//auto point_cloud = std::make_shared<open3d::geometry::PointCloud>();
		//for (const auto& i : ground)
		//{
		//	point_cloud->points_.push_back(points[i]);
		//	point_cloud->colors_.emplace_back(0, 0, 255);
		//}
		//open3d::visualization::DrawGeometries({ point_cloud });

		std::vector<Eigen::Vector3d> scan_points;
		
		std::unordered_set<int> ground_set{ ground.begin(),ground.end() };
		for (int i = 0; i < points.size(); i++)
		{
			if (ground_set.find(i) == ground_set.end())
				scan_points.push_back(points[i]);
		}

		DBSCAN scan = DBSCAN(DBSCAN::Para(0.3));
		auto category = scan.fit(scan_points);
		open3d::utility::Timer random;
		open3d::utility::UniformRandInt(0, 255);

		std::vector< Eigen::Vector3d> colors{ { 255,0,0 },{ 0,255,0 },{ 0,255,255 },{ 255,255,0 }
		};

		Eigen::Vector3d red{ 255,0,0 };
		Eigen::Vector3d green{ 0,255,0 };
		int i = 0;
		//klh_utility::Random<int> random(0,255);
		for (const auto& c : category) {

			Eigen::Vector3d color{ 0,0,0 };
			if (c.size() > 5)
			{
				i++;
				color = colors[i%4];
			}
			for (const auto& p : c) {
				
				point_cloud->points_.push_back(scan_points[p]);
				point_cloud->colors_.push_back(color);
			}
		}

		open3d::visualization::DrawGeometries({ point_cloud });


		//std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> temp_points;
		//temp_points.reserve(points.size());
		//for (const auto& p : points->points_)
		//{
		//	temp_points.push_back(p);
		//}
	}

}


int main()
{
	//LessonThree();
	//LessonSecond();
	LessonFourth();
    std::cout << "Hello World!\n";
	std::getchar();
 }
